/* Math, Drawing, and other Utility functions/classes that don't have a better home
 *
 *
 */
void drawStars(){
    int res = 50;
    stroke(100);
    noFill();
    fill(100);
    
    int xoff = (int)-cameraX/4 % res;
    int yoff = (int)-cameraY/4 % res;
    float jval = -cameraY+1.2*height+2+res;
    float kval = -cameraX+1.2*width+2+res;

//OPENGL drawing - should be quicker    

    if (graphicsMode.equals(OPENGL)){
      res = 50;
      xoff = (int)-cameraX/4 % res;
      yoff = (int)-cameraY/4 % res;
      jval = 1.2*height+2+res;
      kval = 1.2*width+2+res;
   
      gl.glPointSize(3);
//      gl.glEnable(GL_POINT_SMOOTH);
      gl.glBegin(GL.GL_POINTS);
      gl.glColor3f(75/255.0,75/255.0,75/255.0);
      for (float j=-height; j<jval;j+=res){
        for (float k=-width; k<kval;k+=res){
//              gl.glColor4f(100, 100, 100, 255);
              gl.glVertex3f(k-xoff,j-yoff,-415.6922);
        }
      }    
      res = 75;
      xoff = (int)(-cameraX/2.666) % res;
      yoff = (int)(-cameraY/2.666) % res;
      jval = 1.2*height+2+res;
      kval = 1.2*width+2+res;
//      gl.glPointSize(2);      
      gl.glColor3f(125/255.0,125/255.0,125/255.0);
      for (float j=-height; j<jval;j+=res){
        for (float k=-width; k<kval;k+=res){
//              gl.glColor4f(150, 150, 150, 255);
              gl.glVertex3f(k-xoff,j-yoff,-415.6922);
        }
      }    
      res = 100;
      xoff = (int)(-cameraX/2) % res;
      yoff = (int)(-cameraY/2) % res;
      jval = 1.2*height+2+res;
      kval = 1.2*width+2+res;
//      gl.glPointSize(3);
      gl.glColor3f(200/255.0,200/255.0,200/255.0);
      for (float j=-height; j<jval;j+=res){
        for (float k=-width; k<kval;k+=res){
//              gl.glColor4f(255, 255, 255, 255);
              gl.glVertex3f(k-xoff,j-yoff,-415.6922);
        }
      }    
      gl.glEnd(); 
    }
    else{
      //NON-OPENGL
      
    for (float j=-cameraY-height; j<jval;j+=res){
      for (float k=-cameraX-width; k<kval;k+=res){
          point(k-xoff,j-yoff);//
          //        point(pointTransform(k-xoff, j-yoff).x,pointTransform(k-xoff, j-yoff).y);
      }
    }
    
    res = 75;
    stroke(150);
    xoff = (int)(-cameraX/2.666 )%res;
    yoff = (int)(-cameraY/2.666 )%res;
    jval = -cameraY+1.2*height+2+res;
    kval = -cameraX+1.2*width+2+res;
    for (float j=-cameraY-height; j<jval;j+=res){
      for (float k=-cameraX-width; k<kval;k+=res){
//        ellipse(k-xoff,j-yoff,1.1,1.1);//
          point(k-xoff,j-yoff);//
          //else line(k-xoff,j-yoff,k-xoff+1,j-yoff+1);
//        point(pointTransform(k-xoff, j-yoff).x,pointTransform(k-xoff, j-yoff).y);
      }
    }
    res = 100;
    stroke(255);
    xoff = (int)-cameraX/2 %res;
    yoff = (int)-cameraY/2 %res;
    jval = -cameraY+1.2*height+2+res;
    kval = -cameraX+1.2*width+2+res;
    for (float j=-cameraY-height; j<jval;j+=res){
      for (float k=-cameraX-width; k<kval;k+=res){
//        ellipse(k-xoff,j-yoff,1,1);//
          point(k-xoff,j-yoff);
//          point(pointTransform(k-xoff, j-yoff).x,pointTransform(k-xoff, j-yoff).y);
          //else line(k-xoff,j-yoff,k-xoff+1,j-yoff+1);
      }
    }
    }
    
  noStroke();
}

void handleCamera(){
      //deal with camera
    float camDistX = -myPlayer.getParticle(0).position.x + width/2 - cameraX; //how far behind the camera is
    float camDistY = -myPlayer.getParticle(0).position.y + height/2 - cameraY;
//    println(camDistX);
//    println(cameraX);
//    println(myPlayer.getParticle(0).position.x);
    
    if ( Math.abs(camDistX) > width/3.0 ){ cameraX += camDistX- (( camDistX > 0 )?width/3.0:-width/3.0); }
    if ( Math.abs(camDistY) > height/3.0 ){ cameraY += camDistY-(( camDistY > 0 )?height/3.0:-height/3.0); }
      
  //else {myXMultiplier *= 1.0/1.005; if (myXMultiplier < 1.0) {myXMultiplier = 1.0;}}
    //if ( Math.abs(camDistY) > height/3.0 ){ myYMultiplier *= 1.005; }
    //else {myYMultiplier *= 1.0/1.005; if (myYMultiplier < 1.0) {myYMultiplier = 1.0;}}
    
    cameraX = -camFollowSpeed*myXMultiplier*(myPlayer.getParticle(0).position.x - width/2) + (1.0-camFollowSpeed*myXMultiplier)*cameraX;
    cameraY = -camFollowSpeed*myYMultiplier*(myPlayer.getParticle(0).position.y - height/2) + (1.0-camFollowSpeed*myYMultiplier)*cameraY;
    cameraZ = 0;
}


void calcAngularVelocity(){
//  debugCounter++;
  Particle p0 = myPlayer.getParticle(0);
  Particle p1 = myPlayer.getParticle(1);
  Particle p2 = myPlayer.getParticle(2);
  Particle p3 = myPlayer.getParticle(3);
  p0.setVel(); 
  p1.setVel(); 
  p2.setVel(); 
  p3.setVel();
  Vector3d rad = p1.position.minus(p0.position);
  Vector3d vel = p1.velocity.minus(p0.velocity);
  float angVelocity = rad.cross(vel).z / rad.norm();
  rad = p2.position.minus(p0.position);
  vel = p2.velocity.minus(p0.velocity);
  angVelocity += rad.cross(vel).z/rad.norm();
  rad = p3.position.minus(p0.position);
  vel = p3.velocity.minus(p0.velocity);
  angVelocity += rad.cross(vel).z/rad.norm();
  angVelocity *= 1.0/3.0;
  angularVelocity = angVelocity;
  smoothedAngularVelocity *= .9; 
  smoothedAngularVelocity += (angularVelocity*.1);
  
//  DEBUGME - need to set this so we can choose the right parameters for each level
  myPlayer.setSpringStrength( springConstant + (MAX_SPRING_CONSTANT-springConstant)*abs( min(1.0,smoothedAngularVelocity/10000.0) ) );
  //println(smoothedAngularVelocity);
}

//OPT: right now, this is pretty expensive...can we do better?
//Also, need to change 'factor' so that it is less sensitive to small rotations and more sensitive to large ones - might be fixed now
Vector3d pBuff = new Vector3d(0,0,0); //reserved for pointTransform
Vector3d pBuff2 = new Vector3d(0,0,0); //reserved for pointTransform
Vector3d pointTransform(float xIn, float yIn){
  //return new Vector3d(xIn,yIn,0);
  Particle p0 = myPlayer.getParticle(0);
//    Vector3d myDisp = p0.position.minus(new Vector3d(xIn,yIn,0)).times(-1);
  Vector3d myDisp = pBuff;
  myDisp.set(p0.position);
  pBuff2.set(xIn,yIn,0);
  myDisp.subtract(pBuff2);
  myDisp.multiply(-1.0);
  float oldSAV = smoothedAngularVelocity;
  smoothedAngularVelocity = -smoothedMouseForce*.4 + oldSAV * .6;
  float distance = myDisp.norm();
  float myAlpha = (Math.abs(smoothedAngularVelocity)/(200+Math.abs(smoothedAngularVelocity)));
  float factor = 1.0 * (1.0-myAlpha) + ( (5*distance)/( (5*distance) +2*Math.abs(.2*smoothedAngularVelocity)+1))*(myAlpha);
  myDisp.multiply(factor*.3+1*.7);
  float sign;
  if (smoothedAngularVelocity > 0) {
    sign = 1.0;
  } 
  else {
    sign = -1.0;
  }
  float angle = -2.0*sign * (1.0 - factor) / ( distance / 300.0 + 1);
  float newX = (float)(myDisp.x * Math.cos(angle) + myDisp.y * Math.sin(angle));
  float newY = (float)(-myDisp.x * Math.sin(angle) + myDisp.y * Math.cos(angle));
  myDisp.x = newX; 
  myDisp.y = newY;
  smoothedAngularVelocity = oldSAV;
  return p0.position.plus(myDisp);
  
}


//Particle tempParticle; //use for better grabbing
void handleStrutAndParticle(Particle strutA, Particle strutB, Particle testParticle, Strut myStrt){
  if (collideTest(testParticle,strutA.position,strutB.position)){
    if (testParticle.myColor.x > 200){     redDStruts.add(new Strut(testParticle,strutA,REDINDEX));
                                           redDStruts.add(new Strut(testParticle,strutB,REDINDEX)); 
                                           if (rnodrawStrut == null) {myStrt.setDraw(false); myStrt.setApply(false); rnodrawStrut = myStrt;}
                                       }
    else if (testParticle.myColor.y > 200){greenDStruts.add(new Strut(testParticle,strutA,GREENINDEX));
                                           greenDStruts.add(new Strut(testParticle,strutB,GREENINDEX)); 
                                           if (gnodrawStrut == null) {myStrt.setDraw(false); myStrt.setApply(false); gnodrawStrut = myStrt;}
                                       }
    else if (testParticle.myColor.z > 200){blueDStruts.add(new Strut(testParticle,strutA,BLUEINDEX));
                                           blueDStruts.add(new Strut(testParticle,strutB,BLUEINDEX)); 
                                           if (bnodrawStrut == null) {myStrt.setDraw(false); myStrt.setApply(false); bnodrawStrut = myStrt;}
                                       }
  }
}

final boolean lineCross(Vector3d wallA, Vector3d wallB, Vector3d lineA, Vector3d lineB){
    float x1 = lineA.x;
    float y1 = lineA.y;
    float x2 = lineB.x;
    float y2 = lineB.y;
    float x3 = wallA.x; 
    float y3 = wallA.y;
    float x4 = wallB.x; 
    float y4 = wallB.y;

    float ua = ( (x4-x3)*(y1-y3)-(y4-y3)*(x1-x3) );
    float ub = ( (x2-x1)*(y1-y3)-(y2-y1)*(x1-x3) );
    float denom = (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1);
    if (abs(denom)<.0001){      
      return false; //skip town if things get tough
    }
    ua /= denom; 
    ub /= denom;

    if ( (0<ua)&&(ua<1)&&(0<ub)&&(ub<1) ){
      return true;
    }
    
    return false;
}


  final boolean colliding(Particle particle,Vector3d wallA, Vector3d wallB){
    //formula from http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
    //This is a line-cross test, which might not be the best one to use.
    //Vector3d distToWall = particle.position.displacementToLine(wallA,wallB);
    //    Vector3d distToWallLast = particle.positionLast.displacementToLine(wallA,wallB);
    //    Vector3d dirToWall = distToWallLast.normalized();

    float x1 = particle.position.x;
    float y1 = particle.position.y;
    float x2 = particle.positionLast.x;
    float y2 = particle.positionLast.y;
    float x3 = wallA.x; 
    float y3 = wallA.y;
    float x4 = wallB.x; 
    float y4 = wallB.y;

    float ua = ( (x4-x3)*(y1-y3)-(y4-y3)*(x1-x3) );
    float ub = ( (x2-x1)*(y1-y3)-(y2-y1)*(x1-x3) );
    float denom = (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1);
    if (abs(denom)<.0001){      
      return false;
    }
    ua /= denom; 
    ub /= denom;

    if ( (0<ua)&&(ua<1)&&(0<ub)&&(ub<1) ){

      return true;
    }

    return false;
  }

  final boolean collideTest(Particle particle, Vector3d wallA, Vector3d wallB){
//    debugCounter++;
  //THIS IS NOT THE CORRECT CODE, UPDATE BASED ON SOLIDWALL2D!!!
    if (wallA.distanceTo(wallB) < .001){return false;} //if wallA and wallB are the same point, the collide test goes nutso - should never happen if we design well, but if it does, bail out!
    float speed = abs(particle.position.x-particle.positionLast.x)+abs(particle.position.y-particle.positionLast.y);
    float factor = 2*speed+3.0*particle.radius;//This may need to be tweaked to make sure we don't stupidly miss some collisions...
    if ( (particle.positionLast.x < (min(wallA.x,wallB.x)-factor)) || (particle.positionLast.x > (max(wallA.x,wallB.x)+factor) ) ){return false;}
    if ( (particle.positionLast.y < (min(wallA.y,wallB.y)-factor)) || (particle.positionLast.y > (max(wallA.y,wallB.y)+factor) ) ){return false;}
    Vector3d myDisp = new Vector3d(particle.positionLast.displacementToLine(wallA,wallB).normalized().times(particle.radius));
    float origx = particle.position.x;
    float origy = particle.position.y;
    particle.position.add(myDisp);
    if(!colliding(particle,wallA,wallB)){
      particle.position.set(origx,origy,0);
      //float distToWall = particle.position.distanceToLine(wallA,wallB);
      //if (distToWall < particle.radius){return true;}
      return false;
    }
    particle.position.set(origx,origy,0);
    return true;
  }  


//Useful drawing functions
final void circle(float x, float y, float radius){
  ellipse(x,y,2*radius,2*radius);
}
final void circle(Vector3d p, float r){
  ellipse(p.x,p.y,2*r,2*r);
}

final void fakeSphere(float x, float y, float z,float r){
//Quick and dirty "sphere" made up of two circles
  if (sphere3d){
    pushMatrix();
    translate(x,y,z);
    ellipse(0,0,2*r,2*r);
/*    pushMatrix();
    rotateY(3.14/2.0);
    ellipse(0,0,2*r,2*r);
    popMatrix();
*/    popMatrix();
  }
  else{
    ellipse(x,y,2*r,2*r);
  }
}

final void fakeSphere(Vector3d pos, float r){
  fakeSphere(pos.x,pos.y,pos.z,r);
}

void rotate2d(Vector3d myVec, float angle){
    float newX = (float)(myVec.x * Math.cos(angle) + myVec.y * Math.sin(angle));
    float newY = (float)(-myVec.x * Math.sin(angle) + myVec.y * Math.cos(angle));
    myVec.x = newX; myVec.y = newY;
}
